package robocup.component.worldmodel;

import java.util.Arrays;

import robocup.component.SConf;
import robocup.component.WorldModel;
import robocup.component.geometry.Vektor;
import robocup.component.infotypes.InfoReceiver;
import robocup.component.infotypes.PlayModeInfo;
import robocup.component.infotypes.PlayerSayInfo;
import robocup.component.infotypes.PlayerTypesInfo;
import robocup.component.infotypes.RefereeInfo;
import robocup.component.infotypes.SenseBodyInfo;
import robocup.component.infotypes.VisualInfo;
import robocup.component.math.RCMath;


/**
 * @author Thomas Karbe
 *
 */
public class ViewModel
    implements InfoReceiver {  

  private final int numberSlicesExp = 5;
  private final double[] sliceHistory = new double[(int) Math.pow(2, this.numberSlicesExp)];
  private final WorldModel world;
  private double areaMax = 0.;
  private final Vektor lastPos = new Vektor(Vektor.XY, 1000, 1000);
  private final double minPosDiff = 5.0;
  private final double[] estimatedSliceAreas = new double[(int) Math.pow(2, this.numberSlicesExp)];
  private double maxDistInField = 1000;

  /**
   * Creates a new ViewModel
   * @param world
   */
  public ViewModel(final WorldModel world) {
    this.world= world;
    // set each slice to "not seen since 10 cylces"
    Arrays.fill(this.sliceHistory, 10.0);
    // all slices have the same size
    Arrays.fill(this.estimatedSliceAreas, 1);
    this.maxDistInField = 2 * Math
        .sqrt(SConf.getInstance().half_length * SConf.getInstance().half_length + SConf.getInstance().half_width * SConf.getInstance().half_width);

  }


  /**
   * For debugging.
   * 
   */
  public void showHistory() {
  
    System.out
        .print("History (slices=" + Math.pow(2, this.numberSlicesExp) + ")");
    for (final double d : this.sliceHistory) {
      System.out.print(":" + ((double) Math.round(d * 100) / 100));
    }
    System.out.println();
  }
  
  
  /**
   * Returns an array of values proportional to the size of the slices
   * @return the slice areas
   */
  public double[] getEstimatedSliceAreas() {
  
    return estimatedSliceAreas;
  }


  /**
   * returns the age of a direction.
   * 
   * @param angleAbs
   * @return
   * 
   * TODO
   * 
   * fürs passen wäre es schöner, wenn auch das Alter benachbarter Slices mit
   * eingehen würde (geht in die bewertungsfunktion ein) (rw)
   */
  public double getAgeOfDir(final double angleAbs) {
  
    return this.sliceHistory[this.getSliceContainingAbsAngle(angleAbs)];
  }

  /**
   * gives the age of a point
   * 
   * @param point
   *          the point
   * @return the age
   */
  public double getAgeOfPoint(final Vektor point) {
  
    return this.sliceHistory[this.getSliceContainingPoint(this.world
        .getMyPosition(), point)];
  }

  /**
   * Gives the number of the left slice
   * @param slice the neighbour slice
   * @return the left neighbour
   */
  public int getLeftSliceNeighbour(final int slice) {
  
    int neighbour = slice - 1;
    if (neighbour < 0) {
      neighbour += this.getNumberSlices();
    }
    return neighbour;
  }

  /**
   * The number of slices.
   * 
   * @return number of slices
   */
  public int getNumberSlices() {
  
    return (int) Math.pow(2, this.numberSlicesExp);
  }

  /**
   * Gives the number of the right neighbour slice
   * @param slice the neighbour
   * @return the right nieghbour
   */
  public int getRightSliceNeighbour(final int slice) {
  
    int neighbour = slice + 1;
    if (neighbour >= this.getNumberSlices()) {
      neighbour -= this.getNumberSlices();
    }
    return neighbour;
  }

  /**
   * The slice-index containing a abs. angle.
   * 
   * @param angle
   *          the desired angle
   * 
   * @return slice-index containing a abs. angle
   */
  public int getSliceContainingAbsAngle(final double angle) {
  
    return (int) (RCMath.getPositiveNormalizedAngle(angle - 180) / 360.0 * this
        .getNumberSlices());
  }

  /**
   * The slice-index containing a point.
   * 
   * @param pos
   * @param point
   * @return slice-index containing a point
   */
  public int getSliceContainingPoint(final Vektor pos,
      final Vektor point) {
  
    final double angle = pos.getAngleTo(point);
    int value = (int) (angle / this.getSliceWidth()) + this.getNumberSlices() / 2 - 1;
    if (value < 0) {
      value += this.getNumberSlices();
    }
    return value;
  }

  /**
   * @param slice1
   * @param slice2
   * @return the number of slices between slice1 and slice2
   */
  public int getSliceDiff(final int slice1,
      final int slice2) {
  
    return Math.min(Math.abs(slice1 - slice2), this.getNumberSlices() - (Math
        .abs(slice1 - slice2)));
  }

  /**
   * The angle of the left side of an slice. (seen from the center)
   * 
   * @param slice -
   *          the slice (array index)
   * @return normalized angle in degree
   */
  public double getSliceLeftAngle(final int slice) {
  
    if (slice < 0 || slice >= this.getNumberSlices()) {
      throw new IndexOutOfBoundsException();
    }
    return (-180.0 + 360.0 * slice / this.getNumberSlices());
  }

  /**
   * the angle representing the middle between left and right slice side.
   * 
   * @param slice
   * @return the meridian (angle between left and right slice margins)
   */
  public double getSliceMeridian(final int slice) {
  
    return ((this.getSliceLeftAngle(slice) + this.getSliceRightAngle(slice)) / 2.0);
  }

  /**
   * The angle of the right side of an slice. (seen from the center)
   * 
   * @param slice -
   *          the slice (array index)
   * @return normalized angle in degree
   */
  public double getSliceRightAngle(final int slice) {
  
    if (slice < 0 || slice >= this.getNumberSlices()) {
      throw new IndexOutOfBoundsException();
    }
    return (-180.0 + 360.0 * (slice + 1) / this.getNumberSlices());
  }

  /**
   * The width of a slice.
   * 
   * @return width of a slice
   */
  public double getSliceWidth() {
  
    return (360 / Math.pow(2, this.numberSlicesExp));
  }

  /**
   * The % of the slice area that has been seen
   * 
   * @param slice -
   *          the slice (array index)
   * @param left -
   *          left view cone border
   * @param right -
   *          right view cone border
   * @return the percentage of seen area of the slice
   */
  public double overlapAreaPercentage(final int slice,
      double left,
      double right) {
  
    final double sliceArea = 360 / Math.pow(2, this.numberSlicesExp);
    double sliceLeft = this.getSliceLeftAngle(slice);
    double sliceRight = this.getSliceRightAngle(slice);
    // angle crossed the +-180 border
    if (left > right) {
      left = Vektor.normalize(left + 180);
      right = Vektor.normalize(right + 180);
      sliceLeft = Vektor.normalize(sliceLeft + 180);
      sliceRight = Vektor.normalize(sliceRight + 180);
    }
    // no overlap
    if (sliceRight < left || sliceLeft > right) {
      return 0;
    }
    // full overlap (slice in angle)
    if (left <= sliceLeft && right >= sliceRight) {
      return 1;
    }
    // angle inside slice
    if (sliceLeft <= left && sliceRight >= right) {
      return (right - left) / sliceArea;
    }
    // parts of the slice overlap
    final double overlap = Math.min(right - sliceLeft, sliceRight - left);
    if (overlap / sliceArea > 1) {
      System.out
          .println("NeckRotator: OVERLAP > 1 =" + (overlap / sliceArea) + "SA: " + sliceArea + " l" + left + ":r " + right + ":sL " + sliceLeft + ":sR " + sliceRight);
    }
    return (overlap / sliceArea);
  }

  /**
   * estimate the intersecting area of a slice and the field. (only the
   * sliceArea to the next line in slice direction is taken - shouldn't matter)
   * 
   * not original area values are calculated, but a value with the same
   * growth-rate --> same effect
   * 
   */
  private void setEstimatedSliceAreaValues() {
  
    final Vektor pos = this.world.getMyPosition();
    // no major changes to expect
    if (pos.getDistance(this.lastPos) < this.minPosDiff && this.world
        .getCycle() > 0) {
      return;
    }
    // maximum will be new calculated now
    this.areaMax = 0;
  
    // reset
    this.lastPos.copy(pos);
    final double maxArea = this.maxDistInField * this.maxDistInField;
    Arrays.fill(this.estimatedSliceAreas, maxArea);
    double distToTop = SConf.getInstance().half_length - pos.y;
    double distToBottom = SConf.getInstance().half_length + pos.y;
    double distToRight = SConf.getInstance().half_width - pos.x;
    double distToLeft = SConf.getInstance().half_width + pos.x;
  
    // when outside field set to border
    distToTop = Math.max(0, distToTop);
    distToBottom = Math.max(0, distToBottom);
    distToLeft = Math.max(0, distToLeft);
    distToRight = Math.max(0, distToRight);
  
    // the direction of the players nose
    double viewDir;
    // viewDir in radians
    double viewDirRad;
    // distance to Line in dir = viewDir
    double distToLine;
    double triangleArea;
  
    for (int slice = 0; slice < this.estimatedSliceAreas.length; slice++) {
      // set viewDir to the angle in the slice center
      viewDir = this.getSliceMeridian(slice);
      viewDirRad = Math.toRadians(viewDir);
  
      // to top (~0�)
      if (Math.abs(viewDir) < 90) {
        distToLine = distToTop / Math.cos(viewDirRad);
        triangleArea = distToLine * distToLine;
        this.estimatedSliceAreas[slice] = Math.min(
            this.estimatedSliceAreas[slice], Math.abs(triangleArea));
      }
      // to bottom (~+-180�)
      if (Math.abs(viewDir) > 90) {
        distToLine = distToBottom / Math.cos(viewDirRad + Math.PI);
        triangleArea = distToLine * distToLine;
        this.estimatedSliceAreas[slice] = Math.min(
            this.estimatedSliceAreas[slice], Math.abs(triangleArea));
      }
      // to right (~90�)
      if (viewDir > 0) {
        distToLine = distToRight / Math.cos(Math.PI / 2 - viewDirRad);
        triangleArea = distToLine * distToLine;
        this.estimatedSliceAreas[slice] = Math.min(
            this.estimatedSliceAreas[slice], Math.abs(triangleArea));
      }
      // to left (~-90�)
      if (viewDir < 0) {
        distToLine = distToLeft / Math.cos(-Math.PI / 2 - viewDirRad);
        triangleArea = distToLine * distToLine;
        this.estimatedSliceAreas[slice] = Math.min(
            this.estimatedSliceAreas[slice], Math.abs(triangleArea));
      }
      if (this.estimatedSliceAreas[slice] > maxArea) {
        System.out
            .println("Seems that a slice area is bigger then " + "possible, correct Neckrotator");
      }
      // set possibly new maximum
      if (this.areaMax < this.estimatedSliceAreas[slice]) {
        this.areaMax = this.estimatedSliceAreas[slice];
      }
    }
  }

  /**
   * (non-Javadoc)
   * 
   * @see robocup.component.infotypes.InfoReceiver#update(robocup.component.infotypes.PlayerSayInfo)
   */
  public void update(final PlayerSayInfo info) {
  
    // TODO Auto-generated method stub
  
  }

  /**
   * (non-Javadoc)
   * 
   * @see robocup.component.infotypes.InfoReceiver#update(robocup.component.infotypes.PlayModeInfo)
   */
  public void update(final PlayModeInfo info) {
  
    // TODO Auto-generated method stub
  
  }

  /**
   * (non-Javadoc)
   * 
   * @see robocup.component.infotypes.InfoReceiver#update(robocup.component.infotypes.RefereeInfo)
   */
  public void update(final RefereeInfo info) {
  
    // TODO Auto-generated method stub
  
  }

  /**
   * This method should be called when BS-Arrived.
   * 
   * @param info
   *          the sense body info
   * 
   */
  public void update(final SenseBodyInfo info) {
  
    for (int i = 0; i < this.sliceHistory.length; i++) {
      this.sliceHistory[i] += 1;
    }
  }

  /**
   * This method is called when an VI-Arrived and the World has been updated
   * Set's the seen slices.
   * 
   * @param info
   *          the visual info
   * 
   */
  public void update(final VisualInfo info) {
  
    this.updateSeeHistory(this.world.getSelfRef().getHeadDir(), this.world
        .getViewWidth());
    this.setEstimatedSliceAreaValues();
  }

  /**
   * Updates the see-history of the slices (when a slice was last seen).
   * 
   * @param headDir -
   *          abs. headDir
   * @param viewWidth -
   *          view cone width
   */
  private void updateSeeHistory(final double headDir,
      final double viewWidth) {
  
    final double leftBorder = Vektor.normalize(headDir - viewWidth / 2.0);
    final double rightBorder = Vektor.normalize(headDir + viewWidth / 2.0);
    double overlapPercentage;
    // history maximum will be calculated new
    for (int i = 0; i < this.sliceHistory.length; i++) {
      overlapPercentage = this
          .overlapAreaPercentage(i, leftBorder, rightBorder);
      // != 1.0 because every slice should be more interesting than
      // "unseeable" slices (value 0)
      this.sliceHistory[i] *= (1.000001 - overlapPercentage);
    }
  }
  
  /**
   * The exponent that is used to get the full number of slices e.g. 5 for 32 slices 2^5=32
   * @return the exponent
   */
  public int getNumberSlicesExp() {
  
    return numberSlicesExp;
  }


  
  /**
   * Returns the array of history values for each slice
   * @return the history values
   */
  public double[] getSliceHistory() {
  
    return sliceHistory;
  }


  
  /**
   * The biggest area
   * @return the biggest area
   */
  public double getAreaMax() {
  
    return areaMax;
  }


  /** (non-Javadoc)
   * @see robocup.component.infotypes.InfoReceiver#update(robocup.component.infotypes.PlayerTypesInfo)
   */
  public void update(final PlayerTypesInfo info) {

    // TODO Auto-generated method stub
    
  }
  
  
}
